//
// Created by fins on 24-10-6.
//

#ifndef CLOUD_FILTER_FILTER_HPP
#define CLOUD_FILTER_FILTER_HPP

#include "rclcpp/rclcpp.hpp"
#include "lib/cloud_filter.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "std_msgs/msg/string.hpp"
#include "pcl/point_cloud.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <Eigen/Geometry>
#include <pcl_conversions/pcl_conversions.h>
#include "yaml-cpp/yaml.h"
#include "livox_ros_driver2/msg/custom_point.hpp"
#include "livox_ros_driver2/msg/custom_msg.hpp"
#include <chrono>
/***
 * Filter: Filter pointcloud data
 * @function publishMarker: Publish marker
 * @function pointcloud_callback: Pointcloud callback function
 * @function Filter: Constructor
 */
class Filter : public rclcpp::Node {
public:
    Filter();

private:
    void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);

    bool transform_initialized_ = false;
    CloudFilter cloud_filter;
    // 成员变量
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_sub_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr processed_point_cloud_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    geometry_msgs::msg::TransformStamped transform_l2r;
    geometry_msgs::msg::TransformStamped transform_r2l;
    YAML::Node config;
    std::string origin_cloud;
    std::string processed_cloud;
    std::string frame_id_robot;
    std::string frame_id_lidar;
    bool use_box_filter;
    bool transform_to_robot_frame;

};



#endif // CLOUD_FILTER_FILTER_HPP